/*
 * Copyright (C) 2014, Field and Service Robotics 2014, Group 6.
 *
 *  Collision detector
 */

// ROS
#include "ros/ros.h"
// Application
 #include "sensor_msgs/LaserScan.h"

#include "calibration.h"
/*   Setup robot parameters.
 *
 *  @param  ..
 *
 *  @return void
 */

/*  Default constructor.
 *
 */
Calibration::Calibration()
{

}

/*  Main.
 *
 */
int main(int argc, char **argv)
{
    ros::init(argc, argv, "calibration");
    Calibration instance;
    ros::NodeHandle nodeHandle;
    instance.mLaserScanSubscriber = nodeHandle.subscribe("scan", 100, &Calibration::readScan, &instance);

    ros::spin();

    return 0;
}


/*  Callback when a new message has arrived to /scan topic. Reads a laser scan and calculates
 *  whether it is safe to go.
 *
 *  @param  scan  Message from LMS100
 *  @return void
 */
void Calibration::readScan(const sensor_msgs::LaserScan::ConstPtr& scan)
{
    //ROS_INFO("ave");
    double sum = 0.0;
    double ave = 0.0;
    bool found = false;

    for(int i = 260; i < 281; i++)
    {
        sum += scan->ranges[i];
    }
    ave = sum / 21;
    //ROS_INFO("ave = %f", ave);

    for(int i = 260; i < 281; i++)
    {
        ROS_INFO("val %d - %f", i, scan->ranges[i]);
        if(scan->ranges[i] < ave - 0.04)
        {
            ROS_INFO("%d %s", i-270-5, i-270-5 < 0?"-->":"<--");
            return;
        }
    }
    if(!found)
        ROS_INFO("--");

}

